#!/usr/bin/env python
import roslib; roslib.load_manifest('stack_objects')
import rospy
from geometry_msgs.msg import PointStamped
from pr2_tasks.tasks import Tasks
from pr2_tasks.pickplace import PickPlace
from pr2_tasks.tableware_detection import TablewareDetection
from pr2_tasks.pickplace_definitions import PlaceGoal
from pr2_python import base
from pr2_python.world_interface import WorldInterface
from pr2_python.planning_scene_interface import get_planning_scene_interface
from pr2_python.arm_mover import ArmMover
from pr2_python.controller_manager_client import ControllerManagerClient
from pr2_tasks.pickup_tableware import PickupTableware
import copy
import ee_cart_imped_action
from pr2_python import geometry_tools
from geometry_msgs.msg import PoseStamped
from pr2_python import gripper
from pr2_python.arm_mover import ArmMover
from pr2_python.find_free_space import free_spots


def get_free_spot(width=0.4,length=0.6,p=PointStamped()):
   psi = get_planning_scene_interface()
   
   #detect the table and object
   det = TablewareDetection()
   objects = det.detect_objects(point_head_at=p)
   table=objects.table

   #get list of collision objects
   forbidden_list = []
   for pg in objects.pickup_goals:
      box=det._bounding_box_srv(pg.target.cluster)
      box_pos_x = box.pose.pose.position.x - (box.box_dims.x/2.0) - table.x_min
      box_pos_y = box.pose.pose.position.y - (box.box_dims.y/2.0) - table.y_min
      forbidden_list.append(((box_pos_x,box_pos_y),(box.box_dims.x,box.box_dims.y)))
   
   #get the table
   dim_x = table.x_max - table.x_min
   dim_y = table.y_max - table.y_min
   
   spots= free_spots((dim_x,dim_y),[(width,length)],0.02,forbidden_list)
   if spots == None:
      for i in range(5):
         spots= free_spots((dim_x,dim_y),[(width-(i/10.0),length-(i/10.0))],0.02,forbidden_list)
         if spots!=None and len(spots)>0:
            break
   if spots == None:
      rospy.logerr("No free spot found")
      return false
   else:
      rospy.loginfo("Free spot found")
   
   spot_x = spots[0][0] + table.x_min 
   spot_y = spots[0][1] + table.y_min
   spot_z = table.pose.pose.position.z
   
   p=PointStamped()
   p.x= spot_x
   p.y= spot_y
   p.z= spot_z
   p.header.frame_id = '/base_link'
   
   pose=PoseStamped()
   pose.pose.orientation.w=1.0
   pose.header.frame_id='/map'
   pose=transform_point(p,pose)
      
   return pose
